#include <reg167.h>
#include "globals.h"
#include "encoder.h"
#include "config.h"
#include "opt_swingup.h"


#define SIGN(in_val, out_sign)              \
        {                                   \
            if(in_val<0)                    \
            {                               \
                out_sign = -1;              \
            }                               \
            else if(in_val > 0)             \
            {                               \
                out_sign = 1;               \
            }                               \
            else                            \
            {                               \
                out_sign = 0;               \
            }                               \
        }


/* ausserer PDT1 */
// parameter ausseren: a0=0,027389, a1=-0,010389, b0=0,41, b1=-0,39 
const float R1_a0 = 0.027389;
const float R1_a1 = -0.010389;
const float R1_b0 = 0.41;
const float R1_b1 = -0.39;
float Ex0_iso_alt;
float Yx0_iso_alt;

/* Innerer PDT1 */
//  a0=0,018, a1=-0,014, b0=0,41, b1=-0,39
const float R2_a0 = 0.018;
const float R2_a1 = -0.014;
const float R2_b0 = 0.41;
const float R2_b1 = -0.39;
float Ex1_iso_alt = 0x00;
float Yx1_iso_alt = 0x00;

void controller_init() 
{
    T5IC=0x0048;
    T5=15536;      // 10ms / 200ns = 50000 ; 2^16 - 50000 = 15536
    T5CON=0;
    T5R=1;
    
    a0_alt=read_rotaryencoder(0);
}

float swup_x2 = 0;
float swup_x2_iso = 0;

float swup_x2_sign = 0;
float swup_x2_sign_last = 0;
unsigned int swup_x2_tout = 0;

#define SWUP_X2_TOUT_TIME   10

void T5_isr (void) interrupt 0x25 
{
    float Wa1_iso, Ea1_iso, Wa0_iso, Ea0_iso, Ya0_iso,Wx1_iso,Ex1_iso,Yx1_iso,Wx0_iso,Ex0_iso,Yx0_iso ;

    double E_kin, E_pot;


    T5=15536;      // 10ms / 200ns = 50000 ; 2^16 - 50000 = 15536
    
    // HIER DEN OFFSET EINTRAGEN!!
    a0_alt=a0;
    
    // Regeldifferenz 
    a0=-(((read_rotaryencoder(1)-a0_offset+2048) & 0x0FFF)-2048);
    // Winkelgeschwindigkeit
    a1=a0-a0_alt;
    
    // Winkelgeschwindigkeit in RAD
    a0_iso=a0*0.001533981;   // 2*pi / 4096
    a1_iso=a1*0.1533981;     // 100*2*pi / 4096


    /* Aufschwingen des Pendels */
    if(startRegler == 0x00)
    {

    #if (SWUP_ALGO==SWUP_ROCKING_CHAIR)
        x0_iso = x0;  
        
        Wx0_iso = Wx0_iso_Sollpos;
        Ex0_iso = Wx0_iso - x0_iso;
        Yx0_iso =0.0015*Ex0_iso;      
        				
        x1_iso = x1;
        
        Wx1_iso = Yx0_iso;
        Ex1_iso = Wx1_iso - x1_iso;
        Yx1_iso = 85*Ex1_iso;  
        x2_iso = Yx1_iso;
        
        x2=x2_iso*0.000125;

        /*  calculate E_kin and E_pot also in 
            SWUP_ROCKING_CHAIR mode for testing purposes
        */
        E_kin = OptSwingUp_Ekin ((double)a1_iso);
        E_pot = OptSwingUp_Epot ((double)a0_iso);
        E_kin_iso = E_kin;
        E_pot_iso = E_pot;

    #endif /* (SWUP_ALGO==SWUP_ROCKING_CHAIR) */

    #if (SWUP_ALGO==SWUP_ENERGY_MEAS)
        x0_iso = x0;

        x1_iso = x1;

        E_kin = OptSwingUp_Ekin ((double)a1_iso);
        E_pot = OptSwingUp_Epot ((double)a0_iso);

        swup_x2_iso = (float)OptSwingUp_AccDes( (double) a1_iso,            // AngleSpeedAct
                                    (double) a0_iso,            // AngleAct
                                    (double) 0     ,            // DiffCartDesPos
                                    (double) x1_iso,            // CartSpeedAct
                                    (double) E_kin ,            // EKinAct
                                    (double) E_pot              // EPotAct
                                  );

        SIGN(swup_x2_iso,swup_x2_sign);

        if((swup_x2_sign_last != swup_x2_sign) || swup_x2_tout>0)
        {
            x2 = 0;

            if(swup_x2_tout == 0)
            {
                swup_x2_tout = SWUP_X2_TOUT_TIME;
            }
            swup_x2_tout--;
        }
        else
        {
            x2=swup_x2_iso*0.000125;
        }

        swup_x2_sign_last = swup_x2_sign;

        E_kin_iso = E_kin;
        E_pot_iso = E_pot;

    #endif /* (SWUP_ALGO==SWUP_ENERGY_MEAS) */

    }

    /** Regelkreis fuer die Aufrechte Postition */
    else if(startRegler == 0x01)
    {

        x0_iso = x0;  
        
        /** Regelung Schlittenposition */ 
        Wx0_iso = Wx0_iso_Sollpos;
        Ex0_iso = Wx0_iso - x0_iso;
        Yx0_iso =0.0001*Ex0_iso;      
        				
        x1_iso = x1;
        
        /** Regelung Schlittengeschwindgkeit */ 
        Wx1_iso = Yx0_iso;
        Ex1_iso = x1_iso - Wx1_iso;
        Yx1_iso = 0.18*Ex1_iso;       
        
        /** Regelung Winkel */ 
        Wa0_iso = Yx1_iso;
        Ea0_iso = Wa0_iso - a0_iso;
        Ya0_iso = 12*Ea0_iso;    // zweiter P-Regler fuer a0, Kp=18
        
        /** Regelung Winkelgechwindigkeit */
        Wa1_iso = Ya0_iso;
        Ea1_iso = Wa1_iso - a1_iso;
        x2_iso = 30*Ea1_iso;     // innerster P-Regler fuer a1, Kp=50
        
        if ((a0>-200)&&(a0<200)) 
        {
            x2=x2_iso*0.000125;       // 1 / 8000
        } 
        else 
        {
            x1=0;
            x2=0;
        }
    }
}

